/*
* 此文件是hoverboard - firmware - hack项目的一部分。
*
* 版权所有 (C) 2017 - 2018 Rene Hopf <renehopf@mac.com>
* 版权所有 (C) 2017 - 2018 Nico Stute <crinq@crinq.de>
* 版权所有 (C) 2017 - 2018 Niklas Fauth <niklas.fauth@kit.fail>
* 版权所有 (C) 2019 - 2020 Emanuel FERU <aerdronix@gmail.com>
*
* 本程序是自由软件：您可以根据自由软件基金会发布的GNU通用公共许可证的条款，对其进行重新分发和/或修改。
* 许可证的版本可以是第3版，或者（根据您的选择）任何更新的版本。
*
* 本程序是基于希望它能有用的目的而提供的，但没有任何形式的保证，包括适销性或特定用途的适用性的暗示保证。
* 有关更多详细信息，请参阅GNU通用公共许可证。
*
* 您应该已经收到了GNU通用公共许可证的副本。如果没有，请访问 <http://www.gnu.org/licenses/> 查看。
*/

#include <stdio.h>
#include <stdlib.h> // 用于abs()函数
#include "stm32f1xx_hal.h"
#include "defines.h"
#include "setup.h"
#include "config.h"
#include "util.h"
#include "BLDC_controller.h"      /* BLDC的头文件 */
#include "rtwtypes.h"
#include "comms.h"

#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
#include "hd44780.h"
#endif

// 系统时钟配置函数声明
void SystemClock_Config(void);

//------------------------------------------------------------------------
// 外部定义的全局变量
//------------------------------------------------------------------------
// 定时器句柄
extern TIM_HandleTypeDef htim_left;
extern TIM_HandleTypeDef htim_right;
// ADC句柄
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
// ADC缓冲区
extern volatile adc_buf_t adc_buffer;
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
  // LCD句柄和错误标志
  extern LCD_PCF8574_HandleTypeDef lcd;
  extern uint8_t LCDerrorFlag;
#endif

// UART句柄
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;

// UART缓冲区
volatile uint8_t uart_buf[200];

// Matlab定义 - 来自自动代码生成
//---------------
// 块参数
extern P    rtP_Left;                   /* 块参数（自动存储） */
extern P    rtP_Right;                  /* 块参数（自动存储） */
// 外部输出
extern ExtY rtY_Left;                   /* 外部输出 */
extern ExtY rtY_Right;                  /* 外部输出 */
// 外部输入
extern ExtU rtU_Left;                   /* 外部输入 */
extern ExtU rtU_Right;                  /* 外部输入 */
//---------------

// 输入索引
extern uint8_t     inIdx;               // 用于双输入的输入索引
extern uint8_t     inIdx_prev;
// 输入结构体
extern InputStruct input1[];            // 输入结构体
extern InputStruct input2[];            // 输入结构体

// 平均速度
extern int16_t speedAvg;                // 测量的平均速度
// 平均速度绝对值
extern int16_t speedAvgAbs;             // 测量的平均速度绝对值
// 通用超时计数器
extern volatile uint32_t timeoutCntGen; // 通用超时（PPM、PWM、Nunchuk）的超时计数器
// 通用超时标志
extern volatile uint8_t  timeoutFlgGen; // 通用超时（PPM、PWM、Nunchuk）的超时标志
// ADC超时标志
extern uint8_t timeoutFlgADC;           // ADC保护的超时标志：0 = 正常，1 = 检测到问题（线路断开或错误的ADC数据）
// 串口接收超时标志
extern uint8_t timeoutFlgSerial;        // 串口接收命令的超时标志：0 = 正常，1 = 检测到问题（线路断开或错误的接收数据）

// 左右电机PWM值
extern volatile int pwml;               // 左电机PWM的全局变量。范围 -1000 到 1000
extern volatile int pwmr;               // 右电机PWM的全局变量。范围 -1000 到 1000

// 电机使能标志
extern uint8_t enable;                  // 电机使能的全局变量

// 电池电压
extern int16_t batVoltage;              // 电池电压的全局变量

#if defined(SIDEBOARD_SERIAL_USART2)
// 左侧边板串口数据
extern SerialSideboard Sideboard_L;
#endif
#if defined(SIDEBOARD_SERIAL_USART3)
// 右侧边板串口数据
extern SerialSideboard Sideboard_R;
#endif
#if (defined(CONTROL_PPM_LEFT) && defined(DEBUG_SERIAL_USART3)) || (defined(CONTROL_PPM_RIGHT) && defined(DEBUG_SERIAL_USART2))
// PPM捕获值
extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
#endif
#if (defined(CONTROL_PWM_LEFT) && defined(DEBUG_SERIAL_USART3)) || (defined(CONTROL_PWM_RIGHT) && defined(DEBUG_SERIAL_USART2))
// PWM捕获值
extern volatile uint16_t pwm_captured_ch1_value;
extern volatile uint16_t pwm_captured_ch2_value;
#endif


//------------------------------------------------------------------------
// 在main.c中定义的全局变量
//------------------------------------------------------------------------
// 倒车标志
uint8_t backwardDrive;
// 蜂鸣器定时器
extern volatile uint32_t buzzerTimer;
// 主循环计数器
volatile uint32_t main_loop_counter;
// 校准后的电池电压
int16_t batVoltageCalib;         // 校准后电池电压的全局变量
// 电路板温度（摄氏度）
int16_t board_temp_deg_c;        // 校准后电路板温度（摄氏度）的全局变量
// 左电机直流母线电流
int16_t left_dc_curr;            // 左电机直流母线电流的全局变量 
// 右电机直流母线电流
int16_t right_dc_curr;           // 右电机直流母线电流的全局变量 
// 总直流母线电流
int16_t dc_curr;                 // 总直流母线电流的全局变量 
// 左电机命令值
int16_t cmdL;                    // 左电机命令的全局变量 
// 右电机命令值
int16_t cmdR;                    // 右电机命令的全局变量 

//------------------------------------------------------------------------
// 局部变量
//------------------------------------------------------------------------
#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
// 串口反馈结构体
typedef struct{
  uint16_t  start;
  int16_t   cmd1;
  int16_t   cmd2;
  int16_t   speedR_meas;
  int16_t   speedL_meas;
  int16_t   batVoltage;
  int16_t   boardTemp;
  uint16_t  cmdLed;
  uint16_t  checksum;
} SerialFeedback;
// 反馈结构体实例
static SerialFeedback Feedback;
#endif
#if defined(FEEDBACK_SERIAL_USART2)
// 左侧边板LED状态
static uint8_t sideboard_leds_L;
#endif
#if defined(FEEDBACK_SERIAL_USART3)
// 右侧边板LED状态
static uint8_t sideboard_leds_R;
#endif

#ifdef VARIANT_TRANSPOTTER
  // 手柄连接标志
  uint8_t  nunchuk_connected;
  // 设置的距离
  extern float    setDistance;  

  // 检查遥控器标志
  static uint8_t  checkRemote = 0;
  // 距离
  static uint16_t distance;
  // 转向
  static float    steering;
  // 距离误差
  static int      distanceErr;  
  // 上一次距离
  static int      lastDistance = 0;
  // 计数器
  static uint16_t transpotter_counter = 0;
#endif

#ifdef VARIANT_XBBCAR
  // 速度
  int16_t    speed;                // 速度的局部变量。范围 -1000 到 1000
  // 左右电机速度
  int16_t speedL     = 0, speedR     = 0;
  // 上一次左右电机速度
  int16_t lastSpeedL = 0, lastSpeedR = 0;
#else
  // 速度
  static int16_t    speed;                // 速度的局部变量。范围 -1000 到 1000
#endif

#ifndef VARIANT_TRANSPOTTER
  // 转向
  static int16_t  steer;                // 转向的局部变量。范围 -1000 到 1000
  // 转向速率限制器
  static int16_t  steerRateFixdt;       // 转向速率限制器的局部定点变量
  // 速度速率限制器
  static int16_t  speedRateFixdt;       // 速度速率限制器的局部定点变量
  // 转向低通滤波器
  static int32_t  steerFixdt;           // 转向低通滤波器的局部定点变量
  // 速度低通滤波器
  static int32_t  speedFixdt;           // 速度低通滤波器的局部定点变量
#endif

// 上一次蜂鸣器定时器值
static uint32_t    buzzerTimer_prev = 0;
// 无操作超时计数器
static uint32_t    inactivity_timeout_counter;
// 多次点击刹车功能结构体
static MultipleTap MultipleTapBrake;    // 定义刹车踏板的多次点击功能

// 速率
static uint16_t rate = RATE; // 可调节的速率，用于支持启动时的多种驱动模式

#ifdef MULTI_MODE_DRIVE
  // 驱动模式
  static uint8_t drive_mode;
  // 最大速度
  static uint16_t max_speed;
#endif


/**
 * @brief 主函数，程序入口
 * @return int 返回值
 */
int main(void) {

  // HAL库初始化
  HAL_Init();
  // 使能AFIO时钟
  __HAL_RCC_AFIO_CLK_ENABLE();
  // 设置中断优先级分组
  HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
  /* 系统中断初始化 */
  /* MemoryManagement_IRQn中断配置 */
  HAL_NVIC_SetPriority(MemoryManagement_IRQn, 0, 0);
  /* BusFault_IRQn中断配置 */
  HAL_NVIC_SetPriority(BusFault_IRQn, 0, 0);
  /* UsageFault_IRQn中断配置 */
  HAL_NVIC_SetPriority(UsageFault_IRQn, 0, 0);
  /* SVCall_IRQn中断配置 */
  HAL_NVIC_SetPriority(SVCall_IRQn, 0, 0);
  /* DebugMonitor_IRQn中断配置 */
  HAL_NVIC_SetPriority(DebugMonitor_IRQn, 0, 0);
  /* PendSV_IRQn中断配置 */
  HAL_NVIC_SetPriority(PendSV_IRQn, 0, 0);
  /* SysTick_IRQn中断配置 */
  HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);

  // 系统时钟配置
  SystemClock_Config();

  // 禁用DMA1时钟
  __HAL_RCC_DMA1_CLK_DISABLE();
  // GPIO初始化
  MX_GPIO_Init();
  // 定时器初始化
  MX_TIM_Init();
  // ADC1初始化
  MX_ADC1_Init();
  // ADC2初始化
  MX_ADC2_Init();
  // BLDC控制器初始化
  BLDC_Init();        // BLDC控制器初始化

  // 激活锁存
  HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET);   // 激活锁存器
  // 输入限制初始化
  Input_Lim_Init();   // 输入限制初始化
  // 输入初始化
  Input_Init();       // 输入初始化

  // 启动ADC1
  HAL_ADC_Start(&hadc1);
  // 启动ADC2
  HAL_ADC_Start(&hadc2);

  // 开机音效
  poweronMelody();
  // 点亮LED
  HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);

  #ifdef VARIANT_XBBCAR
    // 防止校准前因ADC值异常关机，可在此进入校准模式
    poweroffPressCheck();  // 防止在校准前因不良的ADC值导致关机。可在此处进入校准模式再关机。
    // 检测驾驶模式
    xbbcarDetectDrivingMode();
  #endif
  
  // 电路板温度ADC值固定点滤波输出初始化
  int32_t board_temp_adcFixdt = adc_buffer.temp << 16;  // 固定点滤波器输出，初始化为当前ADC值转换为固定点格式
  // 电路板温度ADC滤波值
  int16_t board_temp_adcFilt  = adc_buffer.temp;

  #ifdef MULTI_MODE_DRIVE
    // 根据ADC值选择驱动模式
    if (adc_buffer.l_tx2 > input1[0].min + 50 && adc_buffer.l_rx2 > input2[0].min + 50) {
      drive_mode = 2;
      max_speed = MULTI_MODE_DRIVE_M3_MAX;
      rate = MULTI_MODE_DRIVE_M3_RATE;
      rtP_Left.n_max = rtP_Right.n_max = MULTI_MODE_M3_N_MOT_MAX << 4;
      rtP_Left.i_max = rtP_Right.i_max = (MULTI_MODE_M3_I_MOT_MAX * A2BIT_CONV) << 4;
    } else if (adc_buffer.l_tx2 > input1[0].min + 50) {
      drive_mode = 1;
      max_speed = MULTI_MODE_DRIVE_M2_MAX;
      rate = MULTI_MODE_DRIVE_M2_RATE;
      rtP_Left.n_max = rtP_Right.n_max = MULTI_MODE_M2_N_MOT_MAX << 4;
      rtP_Left.i_max = rtP_Right.i_max = (MULTI_MODE_M2_I_MOT_MAX * A2BIT_CONV) << 4;
    } else {
      drive_mode = 0;
      max_speed = MULTI_MODE_DRIVE_M1_MAX;
      rate = MULTI_MODE_DRIVE_M1_RATE;
      rtP_Left.n_max = rtP_Right.n_max = MULTI_MODE_M1_N_MOT_MAX << 4;
      rtP_Left.i_max = rtP_Right.i_max = (MULTI_MODE_M1_I_MOT_MAX * A2BIT_CONV) << 4;
    }

    // 打印驱动模式信息
    printf("选择的驱动模式 %i: 最大速度:%i 加速度率:%i \r\n", drive_mode, max_speed, rate);
  #endif

  // 等待按钮释放
  while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }

  #ifdef MULTI_MODE_DRIVE
    // 等待触发释放，超时退出
    int iTimeout = 0;
    while((adc_buffer.l_rx2 + adc_buffer.l_tx2) >= (input1[0].min + input2[0].min) && iTimeout++ < 300) {
      HAL_Delay(10);
    }
  #endif

  // 主循环
  while(1) {
    // 检查蜂鸣器定时器
    if (buzzerTimer - buzzerTimer_prev > 16*DELAY_IN_MAIN_LOOP) {   // 1 ms = 16 个buzzerTimer滴答

      // 读取命令
      readCommand();                        // 读取命令：input1[inIdx].cmd, input2[inIdx].cmd
      // 计算平均速度
      calcAvgSpeed();                       // 计算测量的平均速度：speedAvg, speedAvgAbs

      #ifndef VARIANT_TRANSPOTTER
        // 电机使能条件判断
        if (enable == 0 && !rtY_Left.z_errCode && !rtY_Right.z_errCode && 
            ABS(input1[inIdx].cmd) < 50 && ABS(input2[inIdx].cmd) < 50){
          // 蜂鸣提示电机使能
          beepShort(6);                     // 发出2声蜂鸣声，表示电机已使能
          beepShort(4); HAL_Delay(100);
          // 重置滤波器
          steerFixdt = speedFixdt = 0;      // 重置滤波器
          // 使能电机
          enable = 1;                       // 使能电机
          #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
          // 打印电机使能信息
          printf("-- 电机已使能 --\r\n");
          #endif
        }

        // 计算速度混合值
        #if defined(VARIANT_HOVERCAR) || defined(VARIANT_SKATEBOARD) || defined(ELECTRIC_BRAKE_ENABLE)
          uint16_t speedBlend;                                        // 计算速度混合值，一个介于 [0, 1] 的定点数(0,16,15)
          speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // 速度混合值 [0,1] 范围在 [10 rpm, 60rpm]
        #endif

        // 静止保持功能
        #ifdef STANDSTILL_HOLD_ENABLE
          standstillHold();                                           // 应用静止保持功能。仅适用于电压或转矩模式
        #endif

        // 悬停车模式处理
        #ifdef VARIANT_HOVERCAR
        if (inIdx == CONTROL_ADC) {                                   // 仅当使用踏板（ADC输入）时使用以下实现
          // 检测刹车踏板双击
          if (speedAvgAbs < 60) {                                     // 检查悬停车是否接近静止状态，以启用刹车踏板的双击检测以实现倒车功能
            multipleTapDet(input1[inIdx].cmd, HAL_GetTick(), &MultipleTapBrake); // 此时刹车踏板为 "input1" 变量
          }

          // 刹车踏板处理
          if (input1[inIdx].cmd > 30) {                               // 如果刹车踏板（input1）被按下，同时将油门踏板（input2）归零，以避免“双踏板”驾驶
            input2[inIdx].cmd = (int16_t)((input2[inIdx].cmd * speedBlend) >> 15);
            // 巡航控制
            cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna);         // 如果巡航控制处于激活状态，刹车踏板将其停用
          }
        }
        #endif

        // 电制动功能
        #ifdef ELECTRIC_BRAKE_ENABLE
          electricBrake(speedBlend, MultipleTapBrake.b_multipleTap);  // 应用电制动。仅适用于转矩模式
        #endif

        // 悬停车模式处理
        #ifdef VARIANT_HOVERCAR
        if (inIdx == CONTROL_ADC) {                                   // 仅当使用踏板（ADC输入）时使用以下实现
          // 刹车踏板方向处理
          if (speedAvg > 0) {                                         // 确保刹车踏板方向与运动方向相反，并且在接近静止时归零（以避免通过刹车踏板倒车） 
            input1[inIdx].cmd = (int16_t)((-input1[inIdx].cmd * speedBlend) >> 15);
          } else {
            input1[inIdx].cmd = (int16_t)(( input1[inIdx].cmd * speedBlend) >> 15);
          }
        }
        #endif

        // 滑板车模式处理
        #ifdef VARIANT_SKATEBOARD
          // 油门为负时刹车处理
          if (input2[inIdx].cmd < 0) {                                // 当油门为负时，它作为刹车。此条件确保在接近静止时归零（以避免倒车） 
            // 确保刹车方向与运动方向相反
            if (speedAvg > 0) {                                       // 确保刹车方向与运动方向相反
              input2[inIdx].cmd  = (int16_t)(( input2[inIdx].cmd * speedBlend) >> 15);
            } else {
              input2[inIdx].cmd  = (int16_t)((-input2[inIdx].cmd * speedBlend) >> 15);
            }
          }
        #endif

        // 速率限制和低通滤波
        rateLimiter16(input1[inIdx].cmd, rate, &steerRateFixdt);
        rateLimiter16(input2[inIdx].cmd, rate, &speedRateFixdt);
        filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt);
        filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt);
        // 转换为整数
        steer = (int16_t)(steerFixdt >> 16);  // 将定点数转换为整数
        speed = (int16_t)(speedFixdt >> 16);  // 将定点数转换为整数

        // 悬停车模式处理
        #ifdef VARIANT_HOVERCAR
        if (inIdx == CONTROL_ADC) {               // 仅当使用踏板（ADC输入）时使用以下实现

          #ifdef MULTI_MODE_DRIVE
          // 速度限制
          if (speed >= max_speed) {
            speed = max_speed;
          }
          #endif

          // 检查驾驶方向
          if (!MultipleTapBrake.b_multipleTap) {  // 检查驾驶方向
            speed = steer + speed;                // 前进驾驶：此时 steer = 刹车，speed = 油门
          } else {
            speed = steer - speed;                // 倒车驾驶：此时 steer = 刹车，speed = 油门
          }
          // 不应用转向
          steer = 0;                              // 不应用转向，以避免在STEER_COEFFICIENT不为0时产生副作用
        }
        #endif

        // 坦克转向或混合模式
        #if defined(TANK_STEERING) && !defined(VARIANT_HOVERCAR) && !defined(VARIANT_SKATEBOARD) 
          // 坦克转向（无混合）
          cmdL = steer; 
          cmdR = speed;
        #else 
          // 混合函数
          mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL);   // 此函数实现上述方程
        #endif
        
        #ifdef VARIANT_XBBCAR
          // XBBCAR模式处理
          cmdR = cmdL = xbbcarLoop();
        #endif


        // 设置输出PWM值
        #ifdef INVERT_R_DIRECTION
          pwmr = cmdR;
        #else
          pwmr = -cmdR;
        #endif
        #ifdef INVERT_L_DIRECTION
          pwml = -cmdL;
        #else
          pwml = cmdL;
        #endif
      #endif

      #ifdef VARIANT_TRANSPOTTER
        // 计算距离和转向
        distance    = CLAMP(input1[inIdx].cmd - 180, 0, 4095);
        steering    = (input2[inIdx].cmd - 2048) / 2048.0;
        distanceErr = distance - (int)(setDistance * 1345);

        if (nunchuk_connected == 0) {
          // 计算左右电机命令值
          cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
          cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
          if (distanceErr > 0) {
            // 使能电机
            enable = 1;
          }
          if (distanceErr > -300) {
            #ifdef INVERT_R_DIRECTION
              pwmr = cmdR;
            #else
              pwmr = -cmdR;
            #endif
            #ifdef INVERT_L_DIRECTION
              pwml = -cmdL;
            #else
              pwml = cmdL;
            #endif

            if (checkRemote) {
              if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
                //enable = 1;
              } else {
                // 禁用电机
                enable = 0;
              }
            }
          } else {
            // 禁用电机
            enable = 0;
          }
          // 重置超时计数器和标志
          timeoutCntGen = 0;
          timeoutFlgGen = 0;
        }

        // 超时处理
        if (timeoutFlgGen) {
          // 停止电机
          pwml = 0;
          pwmr = 0;
          // 禁用电机
          enable = 0;
          #ifdef SUPPORT_LCD
            // LCD显示信息
            LCD_SetLocation(&lcd,  0, 0); LCD_WriteString(&lcd, "Len:");
            LCD_SetLocation(&lcd,  8, 0); LCD_WriteString(&lcd, "m(");
            LCD_SetLocation(&lcd, 14, 0); LCD_WriteString(&lcd, "m)");
          #endif
          // 延时
          HAL_Delay(1000);
          // 手柄断开连接
          nunchuk_connected = 0;
        }

        // 距离误差处理
        if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // 错误，机器人太远了！
          // 禁用电机
          enable = 0;
          // 长鸣提示
          beepLong(5);
          #ifdef SUPPORT_LCD
            // LCD清屏
            LCD_ClearDisplay(&lcd);
            // 延时
            HAL_Delay(5);
            // LCD显示信息
            LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "紧急关闭！");
            LCD_SetLocation(&lcd, 0, 1); LCD_WriteString(&lcd, "跟随者速度太快。");
          #endif
          // 关机
          poweroff();
        }

        #ifdef SUPPORT_NUNCHUK
          // 手柄连接检测
          if (transpotter_counter % 500 == 0) {
            if (nunchuk_connected == 0 && enable == 0) {
                if(Nunchuk_Read() == NUNCHUK_CONNECTED) {
                  #ifdef SUPPORT_LCD
                    // LCD显示信息
                    LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "手柄控制");
                  #endif
                  // 手柄连接
                  nunchuk_connected = 1;
	          }
	        } else {
              // 手柄断开连接
              nunchuk_connected = 0;
	        }
          }
        }   
      #endif

      #ifdef SUPPORT_LCD
        // LCD显示信息
        if (transpotter_counter % 100 == 0) {
          if (LCDerrorFlag == 1 && enable == 0) {

          } else {
            if (nunchuk_connected == 0) {
              // 显示距离
              LCD_SetLocation(&lcd,  4, 0); LCD_WriteFloat(&lcd,distance/1345.0,2);
              // 显示设置距离
              LCD_SetLocation(&lcd, 10, 0); LCD_WriteFloat(&lcd,setDistance,2);
            }
            // 显示电池电压
            LCD_SetLocation(&lcd,  4, 1); LCD_WriteFloat(&lcd,batVoltage, 1);
            // LCD_SetLocation(&lcd, 11, 1); LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2);
          }
        }
      #endif
      // 计数器加1
      transpotter_counter++;
    #endif

    // 边板传感器处理
    #if defined(SIDEBOARD_SERIAL_USART2)
      sideboardSensors((uint8_t)Sideboard_L.sensors);
    #endif
    #if defined(FEEDBACK_SERIAL_USART2)
      sideboardLeds(&sideboard_leds_L);
    #endif
    #if defined(SIDEBOARD_SERIAL_USART3)
      sideboardSensors((uint8_t)Sideboard_R.sensors);
    #endif
    #if defined(FEEDBACK_SERIAL_USART3)
      sideboardLeds(&sideboard_leds_R);
    #endif
    

    // 计算电路板温度
    filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
    board_temp_adcFilt  = (int16_t)(board_temp_adcFixdt >> 16);  // 将定点数转换为整数
    board_temp_deg_c    = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;

    // 计算校准后的电池电压
    batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;

    // 计算直流母线电流
    left_dc_curr  = -(rtU_Left.i_DCLink * 100) / A2BIT_CONV;   // 左电机直流母线电流 * 100 
    right_dc_curr = -(rtU_Right.i_DCLink * 100) / A2BIT_CONV;  // 右电机直流母线电流 * 100
    dc_curr       = left_dc_curr + right_dc_curr;            // 总直流母线电流 * 100

    // 调试串口输出
    #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
      if (main_loop_counter % 50 == 0) {    // 每125 ms定期发送数据      
        #if defined(DEBUG_SERIAL_PROTOCOL)
          process_debug();
        #else
          // 打印调试信息
          printf("输入1:%i 输入2:%i 左电机命令:%i 右电机命令:%i 电池ADC:%i 电池电压:%i 温度ADC:%i 温度:%i \r\n",
            input1[inIdx].raw,        // 1: 输入1
            input2[inIdx].raw,        // 2: 输入2
            cmdL,                     // 3: 输出命令: [-1000, 1000]
            cmdR,                     // 4: 输出命令: [-1000, 1000]
            adc_buffer.batt1,         // 5: 用于电池电压校准
            batVoltageCalib,          // 6: 用于验证电池电压校准
            board_temp_adcFilt,       // 7: 用于电路板温度校准
            board_temp_deg_c);        // 8: 用于验证电路板温度校准
        #endif
      }
    #endif

    // 反馈串口输出
    #if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
      if (main_loop_counter % 2 == 0) {    // 每10 ms定期发送数据
        // 填充反馈结构体
        Feedback.start	        = (uint16_t)SERIAL_START_FRAME;
        Feedback.cmd1           = (int16_t)input1[inIdx].cmd;
        Feedback.cmd2           = (int16_t)input2[inIdx].cmd;
        Feedback.speedR_meas	  = (int16_t)rtY_Right.n_mot;
        Feedback.speedL_meas	  = (int16_t)rtY_Left.n_mot;
        Feedback.batVoltage	    = (int16_t)batVoltageCalib;
        Feedback.boardTemp	    = (int16_t)board_temp_deg_c;

        #if defined(FEEDBACK_SERIAL_USART2)
          if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) {
            Feedback.cmdLed     = (uint16_t)sideboard_leds_L;
            Feedback.checksum   = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas 
                                           ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed);

            // 发送反馈数据
            HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Feedback, sizeof(Feedback));
          }
        #endif
        #if defined(FEEDBACK_SERIAL_USART3)
          if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) {
            Feedback.cmdLed     = (uint16_t)sideboard_leds_R;
            Feedback.checksum   = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas 
                                           ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed);

            // 发送反馈数据
            HAL_UART_Transmit_DMA(&huart3, (uint8_t *)&Feedback, sizeof(Feedback));
          }
        #endif
      }
    #endif

    // 关机按钮检测
    poweroffPressCheck();

    // 蜂鸣和紧急关机处理
    if (TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20){  // 在主板过热前关机或电池电量低 3
      #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // 打印关机信息
        printf("正在关机，温度过高\r\n");
      #endif
      // 关机
      poweroff();
    } else if ( BAT_DEAD_ENABLE && batVoltage < BAT_DEAD && speedAvgAbs < 20){
      #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // 打印关机信息
        printf("正在关机，电池电压过低\r\n");
      #endif
      // 关机
      poweroff();
    } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) {                                           // 1声蜂鸣（低音调）：电机错误，禁用电机
      // 禁用电机
      enable = 0;
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (rtY_Left.z_errCode && main_loop_counter % 50 == 0) printf("# 警告: rtY_Left.z_errCode: %i\r\n", rtY_Left.z_errCode);
        // if (rtY_Right.z_errCode && main_loop_counter % 50 == 0) printf("# 警告: rtY_Right.z_errCode: %i\r\n", rtY_Right.z_errCode);
      // #endif
      // 蜂鸣提示
      beepCount(1, 24, 1);
    } else if (timeoutFlgADC) {                                                                       // 2声蜂鸣（低音调）：ADC超时
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (main_loop_counter % 50 == 0) printf("# 警告: ADC值超出范围\r\n");
      // #endif
      // beepCount(2, 24, 1);
      // adc_error_melody();
    } else if (timeoutFlgSerial) {                                                                    // 3声蜂鸣（低音调）：串口超时
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (main_loop_counter % 50 == 0) printf("# 警告: 串口超时\r\n");
      // #endif
      // 蜂鸣提示
      beepCount(3, 24, 1);
    } else if (timeoutFlgGen) {                                                                       // 4声蜂鸣（低音调）：通用超时（PPM、PWM、Nunchuk）
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (main_loop_counter % 50 == 0) printf("# 警告: 通用超时（PPM、PWM、Nunchuk）\r\n");
      // #endif
      // 蜂鸣提示
      beepCount(4, 24, 1);
    } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) {                             // 5声蜂鸣（低音调）：主板温度警告
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (main_loop_counter % 50 == 0) printf("# 警告: STM32正在变热: %4.1f°C\r\n", board_temp_deg_c / 10.0);
      // #endif
      // 蜂鸣提示
      beepCount(5, 24, 1);
    } else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) {                                            // 1声快速蜂鸣（中音调）：电池电量低 1
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (main_loop_counter % 50 == 0) printf("# 警告: 电池电量即将耗尽 1: %4.2fV\r\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
      // #endif
      // 蜂鸣提示
      beepCount(0, 10, 6);
    } else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) {                                            // 1声慢速蜂鸣（中音调）：电池电量低 2
      // #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // if (main_loop_counter % 50 == 0) printf("# 警告: 电池电量即将耗尽 2: %4.2fV\r\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
      // #endif
      // 蜂鸣提示
      beepCount(0, 10, 30);
    } else if (BEEPS_BACKWARD && (((cmdR < -50 || cmdL < -50) && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1声快速蜂鸣（高音调）：电机倒车旋转
      // 蜂鸣提示
      beepCount(0, 5, 1);
      // 倒车标志置位
      backwardDrive = 1;
    } else {  // 不蜂鸣
      // 不蜂鸣
      beepCount(0, 0, 0);
      // 倒车标志清零
      backwardDrive = 0;
    }


    // 无操作超时计数器加1
    inactivity_timeout_counter++;

    // 无操作超时检测
    if (abs(cmdL) > 50 || abs(cmdR) > 50) {
      // 重置无操作超时计数器
      inactivity_timeout_counter = 0;
    }

    #if defined(CRUISE_CONTROL_SUPPORT) || defined(STANDSTILL_HOLD_ENABLE)
      if ((abs(rtP_Left.n_cruiseMotTgt)  > 50 && rtP_Left.b_cruiseCtrlEna) || 
          (abs(rtP_Right.n_cruiseMotTgt) > 50 && rtP_Right.b_cruiseCtrlEna)) {
        // 重置无操作超时计数器
        inactivity_timeout_counter = 0;
      }
    #endif

    if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) {  // 主循环的其余部分可能需要1ms
      #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
        // 打印关机信息
        printf("正在关机，车轮长时间未活动\r\n");
      #endif
      // 关机
      poweroff();
    }


    // HAL_GPIO_TogglePin(LED_PORT, LED_PIN);                 // 这用于通过连接到LED_PIN的示波器测量main()循环的持续时间
    // 更新状态
    inIdx_prev = inIdx;
    buzzerTimer_prev = buzzerTimer;
    main_loop_counter++;
    }
  }
}


// ===========================================================
/**
 * @brief 系统时钟配置函数
 */
void SystemClock_Config(void) {
  RCC_OscInitTypeDef RCC_OscInitStruct;
  RCC_ClkInitTypeDef RCC_ClkInitStruct;
  RCC_PeriphCLKInitTypeDef PeriphClkInit;

  /**
   * 初始化CPU、AHB和APB总线时钟
   */
  RCC_OscInitStruct.OscillatorType      = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState            = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = 16;
  RCC_OscInitStruct.PLL.PLLState        = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource       = RCC_PLLSOURCE_HSI_DIV2;
  RCC_OscInitStruct.PLL.PLLMUL          = RCC_PLL_MUL16;
  // 配置振荡器
  HAL_RCC_OscConfig(&RCC_OscInitStruct);

  /**
   * 初始化CPU、AHB和APB总线时钟
   */
  RCC_ClkInitStruct.ClockType           = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource        = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider       = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider      = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider      = RCC_HCLK_DIV1;

  // 配置时钟
  HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);

  // 外设时钟选择
  PeriphClkInit.PeriphClockSelection    = RCC_PERIPHCLK_ADC;
  // PeriphClkInit.AdcClockSelection    = RCC_ADCPCLK2_DIV8;  // 8 MHz
  PeriphClkInit.AdcClockSelection       = RCC_ADCPCLK2_DIV4;  // 16 MHz
  // 配置外设时钟
  HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);

  /**
   * 配置SysTick中断时间
   */
  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000);

  /**
   * 配置SysTick
   */
  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn中断配置 */
  HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}